#include <iostream>
#include "ut_log_macros.h"
#include "osapi.h"
#include "src/3rd/geos/include/geos/vend/include_nlohmann_json.hpp"
#include "src/3rd/geos/include/geos/vend/json.hpp"
#include "loc_signal_type.h"
#include "src/engine/VDREngine.hpp"
#include "ut_string.hpp"
#include "src/common/ut_signal_format.hpp"
#include "src/common/visual/visual_tools.h"

using namespace std;
class LocationObserverTest :public ILocationObserver{
public:
    int index = 0;
    std::vector< std::shared_ptr<LOC_POS_SIGNAL>> mVecPoses;
    void OnLocationChanged(LocationInfo &locInfo) override {
        std::shared_ptr<LOC_POS_SIGNAL> posSignal =  std::make_shared<LOC_POS_SIGNAL>();
        posSignal->mLocalTickTime = locInfo.mLocalTickTime;
        posSignal->mLongitude = locInfo.m_dLon;
        posSignal->mLatitude = locInfo.m_dLat;
        posSignal->mHeight = locInfo.m_dHeight;
        posSignal->mPitch = locInfo.pitch;
        posSignal->mRoll = locInfo.roll;
        posSignal->mHeading = locInfo.yaw;
        index ++;
        mVecPoses.push_back(posSignal);
    };

    void OnGpsSignalWeak(bool isWeak) override {

    };
};

int main(int argc, char* argv[]) {
    LocationObserverTest locationObserverTest;
    std::vector<std::shared_ptr<LOC_POS_SIGNAL>> rawPosSignal;
    auto tp = std::chrono::system_clock::now();
    std::time_t tt = std::chrono::system_clock::to_time_t(tp);
    char aTime[50] = {0x00};
#ifdef WIN32
    tm time;
    localtime_s(&time, &tt);
    ossnprintf(aTime, 50, "%04d%02d%02d%02d%02d%02d", time.tm_year + 1900, time.tm_mon + 1,
               time.tm_mday, time.tm_hour, time.tm_min, time.tm_sec);
#else
    tm *time = localtime(&tt);
    ossnprintf(aTime, 50, "%04d%02d%02d%02d%02d%02d", time->tm_year + 1900, time->tm_mon + 1,
               time->tm_mday, time->tm_hour, time->tm_min, time->tm_sec);
#endif // DEBUG
    vdr::DREngine::GetInstance()->AddLocationObserver(&locationObserverTest);
    string logPath = std::string(argv[2]);

    VDR_LOG_INIT(logPath.c_str(),"vdr",aTime, true);
    VDR_LOG_DEBUG("vdr","time,lng,lat,height,vn,ve,vd,roll,pitch,yaw,abx,aby,abz,gbx,gby,gbz")
    char* inputLocationFileName = argv[1];
    void *pFile = osFopen(inputLocationFileName, "rb");
    osFseek(pFile, 0, OS_SEEK_END);
    int64_t  m_PosLogBuffSize  = osFtell(pFile);;
    char* m_PosLogBuff = nullptr;
    m_PosLogBuff = (char*) malloc (sizeof(char)*m_PosLogBuffSize);
    osFseek(pFile, 0, OS_SEEK_SET);
    osFread(m_PosLogBuff, 1, m_PosLogBuffSize, pFile);
    char header[5] = {0};
    uint64_t startOffset = 0;
    uint64_t offset = 0;
    bool isFindSignalStart =false;
    char signalData[10240] = {'0'};
    uint32_t  posSignalIdx = 0;
    uint32_t signalIdx = 0;
    const char* pos_flag = "@POS";
    const char* acc_flag = "@ACC";
    const char* gyr_flag = "@GYR";
    const char* car_flag = "@CAR";
    const char* mag_flag = "@MAG";
    const char* rot_flag = "@ROT";
    const char* sat_flag = "@SAT";
    for(auto i=0;i<m_PosLogBuffSize-4;i++){
        if(!isFindSignalStart) {
            memcpy(&header, m_PosLogBuff + offset, 4);
            if (strcmp(header, pos_flag) == 0 ||
                strcmp(header, acc_flag) == 0 ||
                strcmp(header, gyr_flag) == 0 ||
                strcmp(header, mag_flag) == 0 ||
                strcmp(header, rot_flag) == 0 ||
                strcmp(header, car_flag) == 0 ||
                strcmp(header, sat_flag) == 0){
                isFindSignalStart = true;
                startOffset = offset;
            }
        }
        if(isFindSignalStart){
            if(*(m_PosLogBuff + offset) == '\n'){
                memcpy(signalData, m_PosLogBuff + startOffset, offset - startOffset);
                auto signal = vdr::UTSignalFormat::ConvertStringToSignal(signalData);
                vdr::DREngine::GetInstance()->OnSignal(signal);
                if(signal->mLocalSignalType == POS_SIGNAL){
                    std::shared_ptr<LOC_POS_SIGNAL> posSignal = std::static_pointer_cast<LOC_POS_SIGNAL>(signal);
                    rawPosSignal.push_back(posSignal);

                }
                std::fill_n(signalData, 10240, 0);
                isFindSignalStart = false;
            }
        }
        offset++;
    }

    std::string rawPointsVisualFileName = std::string(logPath) + "raw_points.geojson";
    std::string fusionPointsVisualFileName = std::string(logPath) + "fusion_points_"+string(aTime)+".geojson";
    vdr::VisualTools::ParseRawPoints2File(rawPointsVisualFileName.c_str(),rawPosSignal);
    vdr::VisualTools::ParseRawPoints2File(fusionPointsVisualFileName.c_str(),locationObserverTest.mVecPoses);

    return 0;
}
